#include <Arduino.h>
#include <ArduinoOTA.h>
#include <WiFi.h>
#include <stdio.h>

#include "ble_helper.h"
#include "esp_now_helper.h"
#include "simple_string.h"
#include "song.h"

#define LED_PIN 10
// #define A_PIN 2
#define A_PIN 3
#define B_PIN 6
#define C_PIN 7
#define D_PIN 8
#define BEEP_PIN 2
#define A_PWM 5
#define B_PWM 2
#define C_PWM 3
#define D_PWM 4
#define BEEP_PWM 1
#define BEEP_HZ 523
#define MOTOR_HZ 100000

#define TCP_SERVER_ADDR "bemfa.com"

#define TCP_SERVER_PORT "8344"
#define DEFAULT_STASSID "xxxx"
#define DEFAULT_STAPSW "xxxx"
#define UID "xxxx"
#define TOPIC "car"

const char *ntpServer = "pool.ntp.org";
const long gmtOffset_sec = 8 * 3600;
const int daylightOffset_sec = 0;

#define MAX_PACKETSIZE 512
#define KEEPALIVEATIME 30 * 1000
#define LED_IVEATIME 1 * 1000
#define SEND_IVEATIME 3000

WiFiClient TCPclient;
char TcpClient_Buff[MAX_PACKETSIZE + 1] = {0};
unsigned int TcpClient_BuffIndex = 0;
unsigned long preHeartTick = 0;     // 心跳
unsigned long sendTick = 0;         // 发送时间
unsigned long preTCPStartTick = 0;  // 连接
bool preTCPConnected = false;

int last_song_index = -1;
// int song_start_millis = 0;

// 前后
int fw_value = 0;
// 左右
int lr_value = 0;
int beep = 0;
int led_value = 0;

int current_fw_value = 0;
int current_lr_value = 0;
int current_beep = 0;
int current_led_value = 0;

#ifndef __USE_ESP_NOW__
// 连接WIFI
void doWiFiTick();
void startSTA();

// TCP初始化连接
void doTCPClientTick();
void startTCPClient();
void sendtoTCPServer(String p);
#endif

#ifndef __USE_ESP_NOW__
/*
 *发送数据到TCP服务器
 */
void sendtoTCPServer(String p) {
  if (!TCPclient.connected()) {
    Serial.println("Client is not readly");
    return;
  }
  TCPclient.print(p);
  Serial.println("[Send to TCPServer]:String");
  Serial.println(p);
}

/*
 *初始化和服务器建立连接
 */
void startTCPClient() {
  if (TCPclient.connect(TCP_SERVER_ADDR, atoi(TCP_SERVER_PORT))) {
    Serial.print("\nConnected to server:");
    Serial.printf("%s:%d\r\n", TCP_SERVER_ADDR, atoi(TCP_SERVER_PORT));
    char tcpTemp[128];
    sprintf(tcpTemp, "cmd=1&uid=%s&topic=%s\r\n", UID, TOPIC);

    sendtoTCPServer(tcpTemp);
    preTCPConnected = true;
    preHeartTick = millis();
    TCPclient.setNoDelay(true);
  } else {
    Serial.print("Failed connected to server:");
    Serial.println(TCP_SERVER_ADDR);
    TCPclient.stop();
    preTCPConnected = false;
  }
  preTCPStartTick = millis();
}

/*
 *检查数据，发送心跳
 */
void doTCPClientTick() {
  // 检查是否断开，断开后重连
  if (WiFi.status() != WL_CONNECTED) return;

  if (!TCPclient.connected()) {  // 断开重连

    if (preTCPConnected == true) {
      preTCPConnected = false;
      preTCPStartTick = millis();
      Serial.println();
      Serial.println("TCP Client disconnected.");
      TCPclient.stop();
    } else if (millis() - preTCPStartTick > 1 * 1000)  // 重新连接
      startTCPClient();
  } else {
    // timeClient.update();
    while (TCPclient.available()) {  // 收数据
      char c = TCPclient.read();
      TcpClient_Buff[TcpClient_BuffIndex] = c;
      TcpClient_BuffIndex++;

      preHeartTick = millis();
      if ((TcpClient_BuffIndex > 2 &&
           (TcpClient_Buff[TcpClient_BuffIndex - 1] == '\n' &&
            TcpClient_Buff[TcpClient_BuffIndex - 2] == '\r'))) {  // data ready
        TCPclient.flush();
        // printf("Buff:%s\n", TcpClient_Buff);
        // if ((TcpClient_Buff.indexOf("&msg=on") > 0)) {
        //   // turnOnLed();
        // } else if ((TcpClient_Buff.indexOf("&msg=off") > 0)) {
        //   // turnOffLed();
        // }
        str_section sections[4];
        str_section sections2[2];
        str_section sections3[3];
        int count = str_split(TcpClient_Buff, TcpClient_BuffIndex + 1, '&',
                              sections, 4);
        char out_buffer[100];
        char key_buffer[10];
        char value_buffer[100];
        char item_buffer[5];

        for (int i = 0; i < count; i++) {
          // printf("%d:%d\n", sections[i].begin, sections[i].end);
          section_str(sections[i], TcpClient_Buff, out_buffer,
                      sizeof(out_buffer));
          // printf("%s\n", out_buffer);
          int count2 =
              str_split(out_buffer, sections[i].end - sections[i].begin, '=',
                        sections2, 2);
          if (count2 == 2) {
            section_str(sections2[0], out_buffer, key_buffer,
                        sizeof(key_buffer));
            if (strcmp(key_buffer, "msg") == 0) {
              section_str(sections2[1], out_buffer, value_buffer,
                          sizeof(value_buffer));
              parse_command(value_buffer,sizeof(value_buffer));
            }
          }
        }
        memset(TcpClient_Buff, 0, sizeof(TcpClient_Buff));
        TcpClient_BuffIndex = 0;
      }
    }
    if (millis() - preHeartTick >= KEEPALIVEATIME) {  // 保持心跳
      preHeartTick = millis();
      Serial.println("--Keep alive:");
      sendtoTCPServer("ping\r\n");
    }
    // if (millis() - sendTick >= SEND_IVEATIME) {
    //   sendTick = millis();
    //   char msgBuffer[100];
    //   sprintf(msgBuffer, "cmd=2&uid=%s&topic=%s&msg=%d#%d#%d\r\n", UID,
    //   TOPIC,
    //           fw_value, lr_value, beep);
    //   sendtoTCPServer(msgBuffer);
    //   printf("send value\n");
    // }
  }
}

void startSTA() {
  WiFi.disconnect();
  WiFi.mode(WIFI_STA);
  WiFi.begin(DEFAULT_STASSID, DEFAULT_STAPSW);
}

/**************************************************************************
                                 WIFI
***************************************************************************/
/*
  WiFiTick
  检查是否需要初始化WiFi
  检查WiFi是否连接上，若连接成功启动TCP Client
  控制指示灯
*/
void doWiFiTick() {
  static bool startSTAFlag = false;
  static bool taskStarted = false;
  static uint32_t lastWiFiCheckTick = 0;

  if (!startSTAFlag) {
    startSTAFlag = true;
    startSTA();
    Serial.printf("Heap size:%d\r\n", ESP.getFreeHeap());
  }

  // 未连接1s重连
  if (WiFi.status() != WL_CONNECTED) {
    if (millis() - lastWiFiCheckTick > 1000) {
      lastWiFiCheckTick = millis();
    }
  }
  // 连接成功建立
  else {
    if (taskStarted == false) {
      taskStarted = true;
      Serial.print("\r\nGet IP Address: ");
      Serial.println(WiFi.localIP());
      startTCPClient();
      configTime(gmtOffset_sec, daylightOffset_sec, ntpServer);
    }
  }
}
#endif
void parse_command(char *value_buffer, int length) {
  str_section sections[4];
  int count = str_split(value_buffer, length, '#', sections, 4);
  char item_buffer[5];
  if (count == 3 || count == 4) {
    section_str(sections[0], value_buffer, item_buffer, sizeof(item_buffer));
    fw_value = atoi(item_buffer);
    if (count == 3) {
      if (fw_value > 0) {
        led_value = 1;
      } else {
        led_value = 0;
      }
    }
    printf("ble x1:%d\n", fw_value);
    section_str(sections[1], value_buffer, item_buffer, sizeof(item_buffer));
    lr_value = atoi(item_buffer);
    printf("ble x2:%d\n", lr_value);
    section_str(sections[2], value_buffer, item_buffer, sizeof(item_buffer));
    beep = atoi(item_buffer);
    printf("ble x3:%d\n", beep);
    if (count == 4) {
      section_str(sections[3], value_buffer, item_buffer, sizeof(item_buffer));
      led_value = atoi(item_buffer);
      printf("ble x4:%d\n", led_value);
    }
  }
}
#ifdef __USE_ESP_NOW__
void OnDataRecv(const uint8_t *mac_addr, const uint8_t *data, int data_len) {
  parse_command((char *)data, data_len);
}
#endif
void ble_write_callback(std::string value) {
  char *value_buffer = (char *)value.c_str();
  printf("ble_write_callback %s\n", value_buffer);
  parse_command(value_buffer, value.length());
}

void play_song(Song song) {
  last_song_index++;
  if (last_song_index >= song.lenght) {
    last_song_index = 0;
  }
  int hz = song.data[last_song_index * 2];
  if (hz != 0) {
    ledcSetup(BEEP_PWM, hz, 7);
    ledcWrite(BEEP_PWM, 120);
  } else {
    ledcWrite(BEEP_PWM, 0);
  }
  delay(song.data[last_song_index * 2 + 1]);
  ledcWrite(BEEP_PWM, 0);
  delay(10);
}
void stop_song() {
  ledcWrite(BEEP_PWM, 0);
  last_song_index = -1;
}
void doValue() {
  if (beep >= 1 && beep <= numSongs) {
    play_song(allSongs[beep - 1]);
  } else {
    stop_song();
  }
  if (current_lr_value != lr_value) {
    if (lr_value < 0) {
      ledcWrite(A_PWM, lr_value + 20);
      ledcWrite(B_PWM, 0);
    } else if (lr_value > 0) {
      ledcWrite(B_PWM, (-lr_value) + 20);
      ledcWrite(A_PWM, 0);
    } else {
      ledcWrite(A_PWM, 127);
      ledcWrite(B_PWM, 127);
    }
    current_lr_value = lr_value;
  }
  if (current_fw_value != fw_value) {
    if (fw_value > 0) {
      // digitalWrite(LED_PIN, HIGH);
      ledcWrite(C_PWM, fw_value + 20);
      ledcWrite(D_PWM, 0);
    } else if (fw_value < 0) {
      // digitalWrite(LED_PIN, LOW);
      ledcWrite(D_PWM, (-fw_value) + 20);
      ledcWrite(C_PWM, 0);
    } else {
      // digitalWrite(LED_PIN, LOW);
      ledcWrite(BEEP_PWM, 0);
      ledcWrite(C_PWM, 127);
      ledcWrite(D_PWM, 127);
    }
    current_fw_value = fw_value;
  }
  if (led_value != current_led_value) {
    if (led_value > 0) {
      digitalWrite(LED_PIN, HIGH);
    } else {
      digitalWrite(LED_PIN, LOW);
    }
    current_led_value = led_value;
  }
}
void setup() {
  Serial.begin(115200);
  printf("setup  .......   \n");
  pinMode(LED_PIN, OUTPUT);
  pinMode(A_PIN, OUTPUT);
  pinMode(B_PIN, OUTPUT);
  pinMode(C_PIN, OUTPUT);
  pinMode(D_PIN, OUTPUT);
  // pinMode(BEEP_PIN, OUTPUT);
  digitalWrite(A_PIN, LOW);
  digitalWrite(B_PIN, LOW);
  digitalWrite(C_PIN, LOW);
  digitalWrite(D_PIN, LOW);
  // digitalWrite(BEEP_PIN, HIGH);
  delay(10);
  ledcAttachPin(A_PIN, A_PWM);
  ledcAttachPin(B_PIN, B_PWM);
  ledcAttachPin(C_PIN, C_PWM);
  ledcAttachPin(D_PIN, D_PWM);
  ledcAttachPin(BEEP_PIN, BEEP_PWM);

  ledcSetup(A_PWM, MOTOR_HZ, 7);
  ledcSetup(B_PWM, MOTOR_HZ, 7);
  ledcSetup(C_PWM, MOTOR_HZ, 7);
  ledcSetup(D_PWM, MOTOR_HZ, 7);
  // ledcSetup(BEEP_PWM, BEEP_HZ, 7);

  // ledcWrite(BEEP_PWM, 0);
  ledcWrite(A_PWM, 0);
  ledcWrite(B_PWM, 0);
  ledcWrite(C_PWM, 0);
  ledcWrite(D_PWM, 0);
#ifdef __USER_BLE_CONN__
  init_ble(ble_write_callback);
#endif
#ifdef __USE_ESP_NOW__
  WiFi.mode(WIFI_STA);
  configDeviceAP();
  InitESPNow();
  esp_now_register_recv_cb(OnDataRecv);
#endif
  ArduinoOTA.setHostname("esp32car");
  ArduinoOTA.begin();
}
void loop() {
  // printf("test002\n");
  ArduinoOTA.handle();
#ifndef __USE_ESP_NOW__
  doWiFiTick();
  doTCPClientTick();
#endif
  doValue();
}